
//#pragma once

#ifndef COLLISION_DETECTOR_H
#define COLLISION_DETECTOR_H

//#include <iostream>
//#include "Utils\Rotation_utils\RotationUtils.h"

template <typename K> //K is the kernel type
class CollisionDetector{

public:
	/*
	typedef CGAL::Point_2<K>			Point2D;
	typedef CGAL::Polygon_2<K>			Polygon;
	typedef std::vector<Polygon>		Polygon_vector;
	*/
	typedef typename K::FT						NT; //number type
	typedef typename K::Point_2					Point;
	typedef Rotation<NT>						Rotation;
	typedef CGAL::Polygon_2<K>					Polygon;
	typedef Extended_polygon<K>					Robot_rep;
	typedef Reference_point<K>					ReferencePoint;
	typedef std::vector<ReferencePoint>			Path;
	typedef CGAL::Polygon_set_2<K>				Polygon_set;

	
	
private:
	
	NT const			m_step;
	double const		m_tilt;
	Polygon_set			m_obstacles;

	bool isValidRotation(Robot_rep robot,Rotation const& r1, Rotation const& r2) const
	{
		/*double a1 = r1.to_angle();
		double a2 = r2.to_angle();
		if (a1 == a2) return true;
		double a = a1; Rotation r(r1);
		double tilt = (a2>a1) ? m_tilt : -m_tilt;

		while (std::abs(a-a1) < std::abs(a2-a1)){
			robot.rotate_absolute(r);
			if (collide(robot.get_absolute_polygon())) {
				global_graphics->display();
				return false;
			}
			a = a + tilt; r = to_rotation<NT>(a,DEG);
		}
		robot.rotate_absolute(r2);
		if (collide(robot.get_absolute_polygon())) {
			return false;
		}*/
		return true;
	}

	bool isValidTranslation(Robot_rep robot,Point const& s, Point const& t) const
	{
		std::vector<Point> container;
		interpolate(s,t,container);
		if (container.size() < 3) return false;
		for (unsigned int i = 0; i < container.size(); i++){
			robot.translate_absolute(container[i]);
			if (collide(robot.get_absolute_polygon())) {
				return false;
			}
		}
		Robot_rep robo = Robot_rep(robot.get_original_polygon());
		for (unsigned int i = 0; i < container.size(); i++){
			robo.translate_absolute(container[i]);
		}
		return true;
	}

	bool collide(Polygon const & robot) const
	{
		Polygon_set obstacles = m_obstacles;
		return obstacles.do_intersect(robot);
	}



	void interpolate(Point const& s, Point const& t,std::vector<Point>& container) const
	{
		Rotation a = getLineDirection(s,t);
		NT x_step = a.cos()*m_step;
		NT y_step = a.sin()*m_step;

		NT d(distance_points(s,t));
		NT x = s.x(); NT y = s.y(); Point p(s);
		while (distance_points(s,p) < d){
			container.push_back(p);
			x = x + x_step;
			y = y + y_step;
			p = Point(x,y);
		}
		container.push_back(t);
	}

	NT slope(Point const& s, Point const& t) const
	{
		NT dx = t.x()-s.x();
		NT dy = t.y()-s.y();
		return dy/dx;
	}

public:

	double distance_points(Point const& s, Point const& t) const
	{
		NT dx = t.x()-s.x();
		NT dy = t.y()-s.y();
		return std::sqrt(CGAL::to_double(dx*dx+dy*dy));
	}

	CollisionDetector (std::vector<Polygon> const& workspace,NT step=0.05,double tilt=6.0) : 
	m_step(step),m_tilt(tilt)
	{
		for (unsigned int i = 0; i < workspace.size(); i++){
			add_obstacle(workspace[i]);
		}
	}

	bool isValid (Polygon const & robot_model, ReferencePoint const & s, ReferencePoint const & t,Path& path) const
	{
		ReferencePoint next; Rotation r; 
		Robot_rep robot = Robot_rep(robot_model); 
		robot.translate_absolute(s.get_location());
		r = getLineDirection(s.get_location(),t.get_location());

		if (!isValidRotation(robot,s.get_rotation(),r)) return false;
		robot.rotate_absolute(r);
		next = ReferencePoint(s.get_location(),r);
		path.push_back(next);

		if (!isValidTranslation(robot,s.get_location(),t.get_location())) return false;
		robot.translate_absolute(t.get_location());
		next = ReferencePoint(t.get_location(),r);
		path.push_back(next);

		return isValidRotation(robot,r,t.get_rotation());
	}

	Rotation getLineDirection(Point const & s, Point const & t) const
	{
		NT dx = t.x()-s.x();
		NT dy = t.y()-s.y();
		double norm = std::sqrt(CGAL::to_double(dy*dy+dx*dx));
		return Rotation(dy/norm,dx/norm);
	}

	void add_obstacle(Polygon const & polygon)
	{
		//global_graphics->draw_polygon<K>(polygon,BLUE);
		m_obstacles.join(polygon);
	}
};


#endif //COLLOSION_DETECTION_H